package org.hs.phd.odi.bgsub.features;

import java.awt.Polygon;
import java.awt.Rectangle;
import java.util.List;

import org.hs.phd.odi.bgsub.vob.VisualObject;

import com.googlecode.javacv.cpp.opencv_core;
import com.googlecode.javacv.cpp.opencv_core.CvPoint;
import com.googlecode.javacv.cpp.opencv_core.CvRect;
import com.googlecode.javacv.cpp.opencv_core.IplImage;
import com.googlecode.javacv.cpp.opencv_features2d;
import com.googlecode.javacv.cpp.opencv_imgproc;

public class OpenCVFeatureDetector implements FeatureDetector{
	
	private final opencv_features2d.FeatureDetector featureDetector;
	private final opencv_features2d.DescriptorExtractor featureDescriptorExtractor;

	public OpenCVFeatureDetector(opencv_features2d.FeatureDetector featureDetector, opencv_features2d.DescriptorExtractor featureDescriptorExtractor){
		this.featureDetector = featureDetector;
		this.featureDescriptorExtractor = featureDescriptorExtractor;
	}

	@Override
	public void detectAndSetFeatures(IplImage iplImage, List<VisualObject> objects) {
		
		IplImage gray = opencv_core.cvCreateImage(iplImage.cvSize(), iplImage.depth(), 1);
		opencv_imgproc.cvCvtColor(iplImage, gray, opencv_imgproc.CV_BGR2GRAY);
		
		IplImage mask = opencv_core.cvCreateImage(iplImage.cvSize(), iplImage.depth(), 1);
		
		
		for(VisualObject obj: objects){
			Rectangle bounds = obj.getPolygon().getBounds();
			CvRect roiRect = opencv_core.cvRect(bounds.x, bounds.y, bounds.width, bounds.height);
			
			opencv_core.cvSetImageROI(gray, roiRect);

			prepareMask(mask, obj, roiRect);
			
			//opencv_highgui.cvShowImage("mask", mask);
			//opencv_highgui.cvWaitKey();
			
			extractFeatures(gray, mask, obj);
			
		}
		
		opencv_core.cvReleaseImage(mask);
		opencv_core.cvReleaseImage(gray);
	}

	private void extractFeatures(IplImage gray, IplImage mask, VisualObject obj) {
				
		opencv_features2d.KeyPoint keypoints = new opencv_features2d.KeyPoint();
		opencv_core.CvMat descriptors = new opencv_core.CvMat();
		descriptors.setNull();
				
		//TODO there is an opencv bug when no keypoints are detected. See here: https://code.ros.org/trac/opencv/ticket/68
		//I just ignore exceptions in this case
		try{
			featureDetector.detect(gray, keypoints, mask);
			featureDescriptorExtractor.compute(gray, keypoints, descriptors);
			
			if( !descriptors.isNull() ){
				obj.setFeatures(new CvFeatures(keypoints, descriptors));
			}
			
		}catch(RuntimeException ex){
			String message = ex.getMessage();
			if(!message.contains("(-201) Non-positive width or height") &&
			   !message.contains("(-5) Unknown array type"))
			{
				throw ex;
			}
		}

	}

	private void prepareMask(IplImage mask, VisualObject obj, CvRect roiRect) {
		opencv_core.cvSetImageROI(mask, roiRect);			
		opencv_core.cvSet(mask, opencv_core.cvScalarAll(0));
		opencv_core.cvResetImageROI(mask);
		opencv_core.cvFillPoly(mask, getPolygonAsCvPoints(obj.getPolygon()), new int[]{obj.getPolygon().npoints}, 1, opencv_core.cvScalarAll(255), 8, 0);
		opencv_core.cvSetImageROI(mask, roiRect);
	}

	private CvPoint getPolygonAsCvPoints(Polygon polygon) {
		
		int[] xyPoints = new int[2*polygon.npoints];
		
		for(int i = 0; i < polygon.npoints; i++){
			xyPoints[2*i] = polygon.xpoints[i];
			xyPoints[2*i + 1] = polygon.ypoints[i];
		}
		
		CvPoint result = new CvPoint(xyPoints, 0, xyPoints.length);
		
		return result;
	}

	@Override
	public FeatureMatcher getFeatureMatcher() {
		return new OpenCVFeatureMatcher();
	}

}
